#include <stdint.h>
#include <rtthread.h>
#include "canfestival.h"
#include "zy_can.h"
#include "canfestival_can.h"
#include "GQueue.h"

#define DBG_TAG "zy_can"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
rt_sem_t sem_can1Rcv,sem_can2Rcv;
rt_mutex_t pMutex_canOpen;

extern void Canfestival_TimerStart(void);
static Message canRcvBuf[64];
static can_message_t can2RcvBuf[8];
gqueue_t qCanRcv,qCan2Rcv;


//void EnterMutex(void)
//{
//	if(rt_mutex_take(canfstvl_mutex, MAX_MUTEX_WAIT_TIME) != RT_EOK) {
//		LOG_E("canfestival take mutex failed!");
//	}
//}

//void LeaveMutex(void)
//{
//	if(rt_mutex_release(canfstvl_mutex) != RT_EOK) {
//		LOG_E("canfestival release mutex failed!");
//	}
//}


// CANOpen专用接口
unsigned char canSend(CAN_PORT notused, Message *m)
{
    CAN_TxHeaderTypeDef Tx_Header;
	uint32_t TxMailBox;
	uint8_t data[8] = {0};
	Tx_Header.StdId = m->cob_id;
	Tx_Header.IDE = CAN_ID_STD;
	Tx_Header.RTR = m->rtr;
	Tx_Header.DLC = m->len;
	memcpy(data, m->data, m->len);

	if(HAL_CAN_AddTxMessage(&hcan1, &Tx_Header, data, &TxMailBox) != HAL_OK)
	{
		LOG_E("canfestival send failed!");
	};
	
	return 0;
}


int CanTransmit(uint8_t port,can_message_t* m)
{
    CAN_TxHeaderTypeDef Tx_Header;
    uint32_t TxMailBox;
    uint8_t data[8] = {0};
    if(m == NULL){
        return -2;
    }
    if(m->ide){
        Tx_Header.IDE = CAN_ID_EXT;
        Tx_Header.StdId = 0;
        Tx_Header.ExtId = m->id;
    }else{
        Tx_Header.IDE = CAN_ID_STD;
        Tx_Header.StdId = m->id;
        Tx_Header.ExtId = 0;
    }
    Tx_Header.RTR = m->rtr;
    if(m->len <= 8){
        Tx_Header.DLC = m->len;
    }else{
        return -4;
    }
    memcpy(data, m->data, m->len);
    if(port == 1){
        if(HAL_CAN_AddTxMessage(&hcan1, &Tx_Header, data, &TxMailBox) != HAL_OK)
        {
            return -1;
        };
    }else if(port == 2){
        if(HAL_CAN_AddTxMessage(&hcan2, &Tx_Header, data, &TxMailBox) != HAL_OK)
        {
            return -1;
        };
    }else{
        return -3;
    }
    return 0;

}

/**
 * 配置can过滤器
 *
 * @param hcan 
 * @param baudrate 指定波特率
 */
static void CAN_FilterInit(CAN_HandleTypeDef *hcan)
{
	
    /****************************************************************************************
    CAN_FxR1 存储 ID， CAN_FxR2 存储哪个位必须要与 CAN_FxR1 中的
    ID 一致， 2 个寄存器表示 1 组掩码。
                    32位标识符掩码模式下寄存器位划分
    | CAN_FxR1[32:24] |     CAN_FxR1[23:16]     | CAN_FxR1[15:8] |  CAN_FxR1[7:0]       |
    | CAN_FxR2[32:24] |     CAN_FxR2[23:16]     | CAN_FxR2[15:8] |  CAN_FxR2[7:0]       |
    |    STID[10:3]   |  STID[2:0] EXID[17:13]  |    EXID[12:5]  | EXID[4:0] IDE RTR 0  |
    ****************************************************************************************/
	CAN_FilterTypeDef CAN_FilterInitStructure;
	CAN_FilterInitStructure.FilterMode = CAN_FILTERMODE_IDMASK;
	CAN_FilterInitStructure.FilterScale = CAN_FILTERSCALE_32BIT;
	CAN_FilterInitStructure.FilterIdHigh = 0;
	CAN_FilterInitStructure.FilterIdLow = 0;
	CAN_FilterInitStructure.FilterMaskIdHigh = 0;
	CAN_FilterInitStructure.FilterMaskIdLow = 0x4;	// 标准帧֡

	CAN_FilterInitStructure.FilterFIFOAssignment = CAN_RX_FIFO0;
	CAN_FilterInitStructure.FilterActivation = ENABLE;
	CAN_FilterInitStructure.SlaveStartFilterBank = 14;
	if(hcan == &hcan1){
	    CAN_FilterInitStructure.FilterBank = 0;
	}else{
	    CAN_FilterInitStructure.FilterBank = 14;
	}
	if(HAL_CAN_ConfigFilter(hcan, &CAN_FilterInitStructure) != HAL_OK)
	{
		LOG_E("HAL_CAN_ConfigFilter Error_Handler");
		Error_Handler();
	}
}

/**
 * 打开can口
 *
 * @param channel 指定can通道，1或者2
 * @param baudrate 指定波特率

 * @return 0：打开成功；非0：失败
 */
int CAN_Open(uint8_t channel,uint16_t baudrate)
{
	int ret=0;
	CAN_HandleTypeDef* hcan;
	switch(channel)
	{
		case 1:
		    gqueue_init(&qCanRcv,canRcvBuf,sizeof(canRcvBuf)/sizeof(canRcvBuf[0]),sizeof(canRcvBuf[0]));
			hcan = &hcan1;
			hcan->Instance = CAN1;

		break;

		case 2:
		    gqueue_init(&qCan2Rcv,can2RcvBuf,sizeof(can2RcvBuf)/sizeof(can2RcvBuf[0]),sizeof(can2RcvBuf[0]));
            hcan = &hcan2;
            hcan->Instance = CAN2;
		break;
		
		default:
			rt_kprintf("can open parameter error");
			return 1;
	}
	// can的时钟是APB1，42M
	switch(baudrate)
	{
		case 500:
			hcan->Init.Prescaler = 12;
			hcan->Init.Mode = CAN_MODE_NORMAL;
			hcan->Init.SyncJumpWidth = CAN_SJW_1TQ;
			hcan->Init.TimeSeg1 = CAN_BS1_4TQ;
			hcan->Init.TimeSeg2 = CAN_BS2_2TQ;
			hcan->Init.TimeTriggeredMode = DISABLE;
			hcan->Init.AutoBusOff = ENABLE;
			hcan->Init.AutoWakeUp = DISABLE;
			hcan->Init.AutoRetransmission = DISABLE;
			hcan->Init.ReceiveFifoLocked = DISABLE;
			hcan->Init.TransmitFifoPriority = DISABLE;
			break;

		case 1000:
			hcan->Init.Prescaler = 6;
			hcan->Init.Mode = CAN_MODE_NORMAL;
			hcan->Init.SyncJumpWidth = CAN_SJW_1TQ;
			hcan->Init.TimeSeg1 = CAN_BS1_4TQ;
			hcan->Init.TimeSeg2 = CAN_BS2_2TQ;
			hcan->Init.TimeTriggeredMode = DISABLE;
			hcan->Init.AutoBusOff = ENABLE;
			hcan->Init.AutoWakeUp = DISABLE;
			hcan->Init.AutoRetransmission = DISABLE;
			hcan->Init.ReceiveFifoLocked = DISABLE;
			hcan->Init.TransmitFifoPriority = DISABLE;
		break;
		
		default:
			ret = 1;
	}
	
	if (HAL_CAN_Init(hcan) != HAL_OK)
	{
		ret = 2;
		rt_kprintf("can open init failed\r\n");
	}
	
	CAN_FilterInit(hcan);
	if(HAL_CAN_Start(hcan) != HAL_OK)
	{
		ret = 2;
		rt_kprintf("can open start failed\r\n");
		Error_Handler();
	}
	if(HAL_CAN_ActivateNotification(hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK)
	{
		ret = 2;
		rt_kprintf("can open Activate failed\r\n");
		Error_Handler();
	}
	
	return ret;
}

/*
*   CAN接收中断函数, (__weak funtion) received function
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) //CAN Receive
{

    CAN_RxHeaderTypeDef rxHeader;
    uint8_t rxData[8] = {0};
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0,&rxHeader, rxData);
    Message msgRcv;
    can_message_t msgRcv2;
    if(hcan == &hcan1){
        memset(&msgRcv,0,sizeof(Message));
        if(rxHeader.IDE != CAN_ID_STD || rxHeader.DLC > 8)
        {
            return;
        }
        msgRcv.cob_id = rxHeader.StdId;
        msgRcv.len = rxHeader.DLC;
        msgRcv.rtr = rxHeader.RTR;
        memcpy(msgRcv.data,rxData,rxHeader.DLC);
        gqueue_push(&qCanRcv,&msgRcv);
        rt_sem_release(sem_can1Rcv);
    }else if(hcan == &hcan2){
        memset(&msgRcv2,0,sizeof(msgRcv2));
        if(rxHeader.IDE == CAN_ID_STD){
            msgRcv2.ide = 0;
            msgRcv2.id = rxHeader.StdId;
        }else{
            msgRcv2.ide = 1;
            msgRcv2.id = rxHeader.ExtId;
        }
        msgRcv2.rtr = rxHeader.RTR;
        msgRcv2.len = rxHeader.DLC;
        memcpy(msgRcv2.data,rxData,rxHeader.DLC);
        gqueue_push(&qCan2Rcv, &msgRcv2);
    }

    HAL_CAN_ActivateNotification(hcan,CAN_IT_RX_FIFO0_MSG_PENDING);
}

void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan)
{
  UNUSED(hcan);
}

void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan)
{
  UNUSED(hcan);
}


void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan)
{
  UNUSED(hcan);
}


rt_thread_t tid_motor;
extern void Motor_Thread_Entry(void* parameter);
void CAN_Thread_Entry(void * parameter)
{
	Message toCanfestival;
	sem_can1Rcv = rt_sem_create("can1Rcv",0,RT_IPC_FLAG_FIFO);
	pMutex_canOpen = rt_mutex_create("canopen",RT_IPC_FLAG_FIFO);
	if(CAN_Open(1,1000))
	{
		rt_kprintf("CAN_Open failed\r\n");
	}

	Canfestival_TimerStart();
	tid_motor = rt_thread_create("motor",
                                Motor_Thread_Entry,
                                RT_NULL,
                                MOTOR_THREAD_STACK,
                                MOTOR_THREAD_PRIO,
                                MOTOR_THREAD_TIMESLICE);

    if(tid_motor != RT_NULL)
    {
        rt_thread_startup(tid_motor);
    }

	while(1)
	{
		if(rt_sem_take(sem_can1Rcv,RT_WAITING_FOREVER)!= RT_EOK)
		{
			LOG_E("canfestival take canrcv sem failed");
		}
		gqueue_pop(&qCanRcv,&toCanfestival);
		rt_mutex_take(pMutex_canOpen,RT_WAITING_FOREVER);
		canDispatch(&Servo_OD_Data,&toCanfestival);
		rt_mutex_release(pMutex_canOpen);
	}
}



